//----------------------------------------------------------------------------------------------
//	Filename:	PointCloudSceneSolution.h
//	Author:		Sandro Spina
//	Date:		21/06/2013
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------
#pragma once

#include "ModelTraining.h"

#include <Shape\KDTreeSphereMesh.h>
#include <System\Platform.h>
#include <System\Singleton.h>

namespace Illumina
{
	namespace Core
	{
		class PCSUniqueID : 
			public Illumina::Core::TSingleton<PCSUniqueID>
		{
		public:
			::UniqueID UID;

			static int Next(void) 
			{			
				return PCSUniqueID::GetInstance()->UID.GetNext();
			}
		};
		
		class PointCloudSceneSolution
		{
			public:		
				struct SolutionNode;
				struct SolutionMapping {
					std::map<std::string, std::vector<std::pair<float, std::vector<StructureGraph::Node *>>>> * m_solution;
					std::map<std::string, std::vector<FeatureDescStructureGraph::VG_stats*>> * m_vgrids;
					int m_usage_count;
					float m_score;

					std::map<int, std::string> segments_use;

					int ObjectsMatched() { return m_solution->size(); }
					
					bool IsObjectMatched(std::string _tag) { if (m_solution->find(_tag) == m_solution->end()) return false; return true; }

					SolutionMapping() 
					{
						m_solution = new std::map<std::string, std::vector<std::pair<float, std::vector<StructureGraph::Node *>>>>();
						m_vgrids = new std::map<std::string, std::vector<FeatureDescStructureGraph::VG_stats*>>();
						m_usage_count = 1;
						m_score = 0;						
					}

					void AddObjectMatch(std::string _tag, std::pair<float, std::vector<StructureGraph::Node *>> &_match, FeatureDescStructureGraph::VG_stats *_vg_stats)
					{
						std::cout << "--" << _tag << "-- score:" << _match.first << ", #Nodes Used:" << _match.second.size() << " [";
						//First:		Add Segments to the solution.
						(*m_solution)[_tag].push_back(_match);
						(*m_vgrids)[_tag].push_back(_vg_stats);
						
						//Second:		Flag Segments as used along this path.						
						m_score += _match.first;
						for (auto sg : _match.second) { segments_use[sg->id] =  _tag;  std::cout << ":" << sg->id; }
						std::cout << "]" << std::endl;					
					}

					void AddObjectMatch(std::string _tag, std::pair<float, std::vector<StructureGraph::Node *>> &_match)
					{
						std::cout << "--" << _tag << "-- score:" << _match.first << ", #Nodes Used:" << _match.second.size() << " [";
						//First:		Add Segments to the solution.
						(*m_solution)[_tag].push_back(_match);					
						
						//Second:		Flag Segments as used along this path.						
						m_score += _match.first;
						for (auto sg : _match.second) { segments_use[sg->id] =  _tag;  std::cout << ":" << sg->id; }
						std::cout << "]" << std::endl;					
					}

					std::vector<std::pair<float, std::vector<StructureGraph::Node *>>> GetObjectMatched(std::string _tag)
					{
						if (IsObjectMatched(_tag)) 
						{ 
							return m_solution->at(_tag); 
						} 
						else 
						{ 
							std::vector<std::pair<float, std::vector<StructureGraph::Node *>>> empty; 
							return empty; 
						}
					}

					void PrintIMLSolutionScript() 
					{
						//Output .iml file with solution
					}
				};

				struct SolutionNode {
					int id;					
					SolutionMapping * m_mapping;
					SolutionNode * m_parent;

					float m_score;
					int crtActiveNodesCount;
					int crtActiveEdgesCount;
					int crtActivePointsCount;

					std::vector<SolutionNode *> children;

					int AttachChild(SolutionNode * _child) {
						children.push_back(_child);
						return children.size();
					}

					SolutionNode(int _id) {
						id = _id;
						m_parent = NULL;	//Used only for the root of the space.
						m_mapping = new SolutionMapping();												
					}

					SolutionNode(int _id, SolutionNode * _parent) {
						id = _id;
						m_parent = _parent;
						m_mapping = new SolutionMapping();

						crtActiveNodesCount		= m_parent->crtActiveNodesCount;
						crtActiveEdgesCount		= m_parent->crtActiveEdgesCount;
						crtActivePointsCount	= m_parent->crtActivePointsCount;
					}

					//Use this constructor to create a node in the space which shared mapping
					SolutionNode(int _id, SolutionNode * _parent, SolutionMapping * _mapping) {						
						id = _id;
						m_parent = _parent;
						m_mapping = _mapping;

						crtActiveNodesCount		= m_parent->crtActiveNodesCount;
						crtActiveEdgesCount		= m_parent->crtActiveEdgesCount;
						crtActivePointsCount	= m_parent->crtActivePointsCount;
					}

					void AddObjectMatchInMapping(std::string _tag, std::pair<float, std::vector<StructureGraph::Node *>> &_match, FeatureDescStructureGraph::VG_stats * _vgstats)
					{
						m_mapping->AddObjectMatch(_tag, _match, _vgstats);
						crtActiveNodesCount -= _match.second.size();
						for (auto sg : _match.second)
						{
							crtActivePointsCount -= sg->segment->points->size();
							crtActiveEdgesCount -= sg->edges->size();
							m_mapping->segments_use[sg->id] = _tag;
						}
					}

					void AddObjectMatchInMapping(std::string _tag, std::pair<float, std::vector<StructureGraph::Node *>> &_match)
					{
						m_mapping->AddObjectMatch(_tag, _match);
						crtActiveNodesCount -= _match.second.size();
						for (auto sg : _match.second)
						{
							crtActivePointsCount -= sg->segment->points->size();
							crtActiveEdgesCount -= sg->edges->size();
							m_mapping->segments_use[sg->id] = _tag;
						}
					}

					int GetNumberOfChildren() { return children.size(); }

					//Check if segment has been used along this path
					bool SegmentUsed(int _segmentId)
					{
						if (m_mapping->segments_use.find(_segmentId) != m_mapping->segments_use.end()) return true;
						SolutionNode * parent = m_parent;
						while (!parent->IsRoot()) 
						{
							if (parent->m_mapping->segments_use.find(_segmentId) != parent->m_mapping->segments_use.end()) return true;
							parent = parent->m_parent;
						}
						return false;
					}

					bool IsRoot() {						
						if (m_parent==NULL) return true;
						return false; 
					}

					bool IsLeaf() { 
						if (children.size() == 0) return true; 
						return false; 
					}

					SolutionNode * GetBestScoringChild() { return children.front(); }
				};

				struct SolutionSpace {
					SolutionNode * m_root;
					std::vector<SolutionNode *> m_nodes;				
					int size;					

					//Constructor
					SolutionSpace() {						
						m_root = CreateNode();
						size = 1;
					}

					//Methods to Build Up Space
					SolutionNode * CreateNode() {						
						SolutionNode * node = new SolutionNode(PCSUniqueID::Next());						
						m_nodes.push_back(node);
						size++;
						//std::cout << "Created Now Solution Node: " << node->id << " TotalNodes=" << m_nodes.size() << std::endl;
						return node;
					}

					SolutionNode * CreateNode(SolutionNode * _parent) {						
						SolutionNode * node = new SolutionNode(PCSUniqueID::Next(), _parent);
						m_nodes.push_back(node);
						size++;
						_parent->AttachChild(node);
						//std::cout << "Created Now Solution Node: " << node->id << " TotalNodes=" << m_nodes.size() << " with ParentID=" << _parent->id << std::endl;
						return node;
					}

					SolutionNode * CreateNode(SolutionNode * _parent, SolutionMapping * _mapping) {						
						SolutionNode * node = new SolutionNode(PCSUniqueID::Next(), _parent, _mapping);
						m_nodes.push_back(node);
						size++;
						_parent->AttachChild(node);
						//std::cout << "Created Now Solution Node: " << node->id << " TotalNodes=" << m_nodes.size() << " with ParentID=" << _parent->id << std::endl;
						return node;
					}

					//Methods to Traverse Space
					void OrderSolutions() {
					
					}

					void PrintLeafFirstTraversal() {
						for (auto n : m_nodes)
						{
							if (n->IsLeaf())
							{
								SolutionNode * crt = n;
								std::cout << "Solution [Score=" << crt->m_mapping->m_score << "] Path = "; 
								while (true)
								{
									std::cout << crt->id;
									for (auto o = crt->m_mapping->m_solution->begin(); o != crt->m_mapping->m_solution->end(); o++)
									{
										std::cout << " [" << o->first << ":" << o->second.size() << "]";
									}
									std::cout << " -=> ";
									crt = crt->m_parent;
									if (crt->IsRoot()) 
									{ 
										std::cout << crt->id << "." << std::endl; 
										break; 
									}
								}
							}
						}
						std::cout << std::endl;
					}

					std::vector<int> GetBoundaryNodes() {
						std::vector<int> node_ids;
						for (auto n : m_nodes)
						{
							auto surface_vectors = n->m_mapping->m_solution->find("boundary");
							if (surface_vectors != n->m_mapping->m_solution->end())
							{
								for (auto v : surface_vectors->second)
								{
									for (auto str_nodes : v.second)
									{
										node_ids.push_back(str_nodes->id);
									}
								}
							}
						}
						return node_ids;
					}

					void PrintLeafFirstTraversal(int _id) {
						for (auto n : m_nodes)
						{
							if (n->IsLeaf() & n->id == _id)
							{
								SolutionNode * crt = n;
								std::cout << "Solution [Score=" << crt->m_mapping->m_score << "] Path = "; 
								while (true)
								{
									std::cout << crt->id;
									for (auto o = crt->m_mapping->m_solution->begin(); o != crt->m_mapping->m_solution->end(); o++)
									{
										std::cout << " [" << o->first << ":" << o->second.size() << "]";
									}
									std::cout << " -=> ";
									crt = crt->m_parent;
									if (crt->IsRoot()) 
									{ 
										std::cout << crt->id << "." << std::endl; 
										break; 
									}
								}
							}
						}
					}

					std::vector<FeatureDescStructureGraph::VG_stats*> * GetVG_LeafFirstTraversal() {
						std::vector<FeatureDescStructureGraph::VG_stats*> * grids = new std::vector<FeatureDescStructureGraph::VG_stats*>();
						for (auto n : m_nodes)
						{
							if (n->IsLeaf())
							{
								SolutionNode * crt = n;
								std::cout << "Solution [Score=" << n->m_mapping->m_score << "] Path = "; 								
								while (true)
								{
									std::cout << crt->id;
									for (auto o = crt->m_mapping->m_vgrids->begin(); o != crt->m_mapping->m_vgrids->end(); o++)
									{
										for (auto g = o->second.begin(); g != o->second.end(); g++)
										{
											grids->push_back(*g);
											//std::cout << ") VG::" << g->tgrid->m_parent->m_pcs->m_pointCloud->GetName() << ":" << g->tgrid->m_parent->GetInitialNodeId() << " Rot::" << g->best_rot << " UVWMin::" << g->best_min_uvw_t.ToString() << std::endl;
										}
									}
									crt = crt->m_parent;
									if (crt->IsRoot()) break; 									
								}
								std::cout << std::endl;
							}
						}
						return grids;
					}

					void BestFirstTraversal() {

					}

					//Methods for Space Visualisation 
					void PrintIMLSolutionScripts(int _bestCount);
				};

				struct ObjectLocationConstraints {					
					std::map<std::string, std::vector<std::string>> on_map;
					std::map<std::string, std::vector<std::string>> adj_map;
					std::map<std::string, std::vector<std::string>> hang_map;

					ObjectLocationConstraints() {
						on_map["floor"].push_back("chair");
						on_map["floor"].push_back("table");
						on_map["floor"].push_back("sofa");
						on_map["floor"].push_back("armchair");
						on_map["floor"].push_back("officechair");
						on_map["floor"].push_back("cabinet");

						on_map["table"].push_back("monitor");
						on_map["floor"].push_back("keyboard");

						adj_map["wall"].push_back("cabinet");

						hang_map["wall"].push_back("painting");
					}

					bool on(std::string _obj1, std::string _obj2) {
						std::vector<std::string> on_list = on_map["_obj1"];
						for (auto on_list_it = on_list.begin(); on_list_it != on_list.end(); on_list_it++) 
						{
							if (_obj2==*on_list_it) return true;
						}
						return false;
					}

					bool adj(std::string _obj1, std::string _obj2) {
						std::vector<std::string> adj_list = adj_map["_obj1"];
						for (auto adj_list_it = adj_list.begin(); adj_list_it != adj_list.end(); adj_list_it++) 
						{
							if (_obj2==*adj_list_it) return true;
						}
						return false;
					}

					bool hang(std::string _obj1, std::string _obj2) {
						std::vector<std::string> hang_list = hang_map["_obj1"];
						for (auto hang_list_it = hang_list.begin(); hang_list_it != hang_list.end(); hang_list_it++) 
						{
							if (_obj2==*hang_list_it) return true;
						}
						return false;
					}



				};

				struct SceneMapping {
					int id;
					int segment;
					int model;
					int fdg;
					int grid;

					SceneMapping(int _id, int _segment, int _model, int _fdg, int _grid)
					{
						id = _id;
						segment = _segment;
						model = _model;
						fdg = _fdg;
						grid = _grid;
					}


				};
				

				struct vg_history {

					struct history_unit {
						std::vector<int> segs;
						FeatureDescStructureGraph::VoxelGrid* vg;
						float coverage;
						float inclusion_bias;
						float anchor_match_bias;
						float non_anchor_match_bias;
						int pos_match_count;
						int neg_match_count;
						int model_id;
						int grid_idx;
						int fdg_id;
						Vector3 u;
						Vector3 v;
						Vector3 w;
						Vector3 tgrid_origin;

						history_unit(void) { };

						history_unit (std::vector<int> &p_segs, FeatureDescStructureGraph::VoxelGrid * p_vg, Vector3 p_tgrid_origin, float p_coverage, float p_inclusion_bias, float p_anchor_match_bias, float p_non_anchor_match_bias, int p_pos_match_count, int p_neg_match_count, int p_model_id, int p_grid_idx, Vector3 p_u, Vector3 p_v, Vector3 p_w, int p_fdg_id)
						{
							for (auto s: p_segs) segs.push_back(s);
							vg = p_vg;
							tgrid_origin = p_tgrid_origin;
							coverage = p_coverage;
							inclusion_bias = p_inclusion_bias;
							anchor_match_bias = p_anchor_match_bias;
							non_anchor_match_bias = p_non_anchor_match_bias;
							pos_match_count = p_pos_match_count;
							neg_match_count = p_neg_match_count;
							model_id = p_model_id;
							grid_idx = p_grid_idx;
							fdg_id = p_fdg_id;
							u = Vector3(p_u.X, p_u.Y, p_u.Z);
							v = Vector3(p_v.X, p_v.Y, p_v.Z);
							w = Vector3(p_w.X, p_w.Y, p_w.Z);
						}						
					};
					
					std::vector<history_unit> * m_vg_history_0;
					std::vector<history_unit> * m_vg_history_1;
					std::vector<history_unit> * m_vg_history_2;
					std::vector<history_unit> * m_vg_history_3;
					std::vector<history_unit> * m_vg_history_4;
					std::vector<history_unit> * m_vg_history_5;
					std::vector<history_unit> * m_vg_history_6;
					std::vector<history_unit> * m_vg_history_7;

					std::vector<history_unit> * m_vg_history_8;   //Store Best Results here.

					int crt_view_history_0;
					int crt_view_history_1;
					int crt_view_history_2;
					int crt_view_history_3;
					int crt_view_history_4;
					int crt_view_history_5;
					int crt_view_history_6;
					int crt_view_history_7;

					int crt_view_history_8;   //best

					int active_history_line;

					std::vector<std::pair<std::pair<int, float>, Vector3>> * grid_spheres;
					KDTreeSphereMesh * sphereMesh;
					GeometricPrimitive * grid_gp;

					GeometricPrimitive * model_gp;

					int grid_index_with_best_anchor_matches;
					int grid_index_with_best_non_anchor_matches;
					int grid_index_with_best_anchor_plus_non_anchor_matches;
					int grid_index_with_best_coverage;
					float best_coverage_score;
					int best_anchor_match_score;
					int best_non_anchor_match_score;
					int best_anchor_plus_non_score;

					vg_history()
					{
						m_vg_history_8	= new std::vector<history_unit>();
						m_vg_history_0		= new std::vector<history_unit>();
						m_vg_history_1		= new std::vector<history_unit>();
						m_vg_history_2		= new std::vector<history_unit>();
						m_vg_history_3		= new std::vector<history_unit>();
						m_vg_history_4		= new std::vector<history_unit>();
						m_vg_history_5		= new std::vector<history_unit>();
						m_vg_history_6		= new std::vector<history_unit>();
						m_vg_history_7		= new std::vector<history_unit>();
						crt_view_history_0 = 0;
						crt_view_history_1 = 0;
						crt_view_history_2 = 0;
						crt_view_history_3 = 0;
						crt_view_history_4 = 0;
						crt_view_history_5 = 0;
						crt_view_history_6 = 0;
						crt_view_history_7 = 0;
						crt_view_history_8 = 0;
						grid_index_with_best_coverage = 0;
						grid_index_with_best_anchor_matches = 0;
						grid_index_with_best_non_anchor_matches = 0;
						grid_index_with_best_anchor_plus_non_anchor_matches = 0;
						best_coverage_score = 0;
						best_anchor_match_score = 0;
						best_non_anchor_match_score = 0;
						best_anchor_plus_non_score = 0;
						grid_spheres = new std::vector<std::pair<std::pair<int, float>, Vector3>>();
						sphereMesh = new KDTreeSphereMesh();
						grid_gp = new GeometricPrimitive();						
						grid_gp->SetShape(sphereMesh);

						model_gp = new GeometricPrimitive();
					}

					void SetActiveHistoryLine(int p_idx)
					{
						if (p_idx < 9)
							active_history_line = p_idx;
						else active_history_line = 8;

						switch (active_history_line) {
							case 8:	crt_view_history_8=0; break;
							case 0:	crt_view_history_0=0; break;
							case 1:	crt_view_history_1=0; break;
							case 2:	crt_view_history_2=0; break;
							case 3:	crt_view_history_3=0; break;
							case 4:	crt_view_history_4=0; break;
							case 5:	crt_view_history_5=0; break;
							case 6:	crt_view_history_6=0; break;
							case 7:	crt_view_history_7=0; break;							

							default: crt_view_history_8=0; break;
						}

					}

					std::vector<history_unit> * GetActiveHistoryLine()
					{
						switch (active_history_line) {
							case 8:	return m_vg_history_8;
							case 0:	return m_vg_history_0;
							case 1:	return m_vg_history_1;
							case 2:	return m_vg_history_2;
							case 3:	return m_vg_history_3;
							case 4:	return m_vg_history_4;
							case 5:	return m_vg_history_5;
							case 6:	return m_vg_history_6;
							case 7:	return m_vg_history_7;

							default: return m_vg_history_8;
						}
					}

					void IncCrtViewActiveHistoryLine(int p_step)
					{						
						switch (active_history_line) {
							case 8:	crt_view_history_8+=p_step; break;
							case 0:	crt_view_history_0+=p_step; break;
							case 1:	crt_view_history_1+=p_step; break;
							case 2:	crt_view_history_2+=p_step; break;
							case 3:	crt_view_history_3+=p_step; break;
							case 4:	crt_view_history_4+=p_step; break;
							case 5:	crt_view_history_5+=p_step; break;
							case 6:	crt_view_history_6+=p_step; break;
							case 7:	crt_view_history_7+=p_step; break;							

							default: crt_view_history_8++; break;
						}
					}

					void BulkIncCrtViewActiveHistoryLine_Coverage()
					{
						switch (active_history_line) {
							case 8:	crt_view_history_8++; break;
							case 0:	crt_view_history_0++; break;
							case 1:	crt_view_history_1++; break;
							case 2:	crt_view_history_2++; break;
							case 3:	crt_view_history_3++; break;
							case 4:	crt_view_history_4++; break;
							case 5:	crt_view_history_5++; break;
							case 6:	crt_view_history_6++; break;
							case 7:	crt_view_history_7++; break;							

							default: crt_view_history_8++; break;
						}

						while ((GetNextCrtViewActiveHistoryLine() < GetActiveHistoryLine()->size()) && 
								GetActiveHistoryLine()->at(GetNextCrtViewActiveHistoryLine()).coverage < best_coverage_score)
						{							
							switch (active_history_line) {
								case 8:	crt_view_history_8++; break;
								case 0:	crt_view_history_0++; break;
								case 1:	crt_view_history_1++; break;
								case 2:	crt_view_history_2++; break;
								case 3:	crt_view_history_3++; break;
								case 4:	crt_view_history_4++; break;
								case 5:	crt_view_history_5++; break;
								case 6:	crt_view_history_6++; break;
								case 7:	crt_view_history_7++; break;

								default: crt_view_history_8++; break;
							}
						}						
					}

					void BulkIncCrtViewActiveHistoryLine_AnchorMatches()
					{
						switch (active_history_line) {
							case 8:	crt_view_history_8++; break;
							case 0:	crt_view_history_0++; break;
							case 1:	crt_view_history_1++; break;
							case 2:	crt_view_history_2++; break;
							case 3:	crt_view_history_3++; break;
							case 4:	crt_view_history_4++; break;
							case 5:	crt_view_history_5++; break;
							case 6:	crt_view_history_6++; break;
							case 7:	crt_view_history_7++; break;							

							default: crt_view_history_8++; break;
						}

						while ((GetNextCrtViewActiveHistoryLine() < GetActiveHistoryLine()->size()) && 
								GetActiveHistoryLine()->at(GetNextCrtViewActiveHistoryLine()).pos_match_count < best_anchor_match_score)
						{							
							switch (active_history_line) {
								case 8:	crt_view_history_8++; break;
								case 0:	crt_view_history_0++; break;
								case 1:	crt_view_history_1++; break;
								case 2:	crt_view_history_2++; break;
								case 3:	crt_view_history_3++; break;
								case 4:	crt_view_history_4++; break;
								case 5:	crt_view_history_5++; break;
								case 6:	crt_view_history_6++; break;
								case 7:	crt_view_history_7++; break;

								default: crt_view_history_8++; break;
							}
						}						
					}

					void BulkIncCrtViewActiveHistoryLine_NonAnchorMatches()
					{
						switch (active_history_line) {
							case 8:	crt_view_history_8++; break;
							case 0:	crt_view_history_0++; break;
							case 1:	crt_view_history_1++; break;
							case 2:	crt_view_history_2++; break;
							case 3:	crt_view_history_3++; break;
							case 4:	crt_view_history_4++; break;
							case 5:	crt_view_history_5++; break;
							case 6:	crt_view_history_6++; break;
							case 7:	crt_view_history_7++; break;							

							default: crt_view_history_8++; break;
						}

						while ((GetNextCrtViewActiveHistoryLine() < GetActiveHistoryLine()->size()) && 
								GetActiveHistoryLine()->at(GetNextCrtViewActiveHistoryLine()).neg_match_count <= best_non_anchor_match_score)
						{							
							switch (active_history_line) {
								case 8:	crt_view_history_8++; break;
								case 0:	crt_view_history_0++; break;
								case 1:	crt_view_history_1++; break;
								case 2:	crt_view_history_2++; break;
								case 3:	crt_view_history_3++; break;
								case 4:	crt_view_history_4++; break;
								case 5:	crt_view_history_5++; break;
								case 6:	crt_view_history_6++; break;
								case 7:	crt_view_history_7++; break;

								default: crt_view_history_8++; break;
							}
						}						
					}

					void BulkIncCrtViewActiveHistoryLine_Non_Plus_AnchorMatches()
					{
						switch (active_history_line) {
							case 8:	crt_view_history_8++; break;
							case 0:	crt_view_history_0++; break;
							case 1:	crt_view_history_1++; break;
							case 2:	crt_view_history_2++; break;
							case 3:	crt_view_history_3++; break;
							case 4:	crt_view_history_4++; break;
							case 5:	crt_view_history_5++; break;
							case 6:	crt_view_history_6++; break;
							case 7:	crt_view_history_7++; break;							

							default: crt_view_history_8++; break;
						}

						while ((GetNextCrtViewActiveHistoryLine() < GetActiveHistoryLine()->size()) && 
								(GetActiveHistoryLine()->at(GetNextCrtViewActiveHistoryLine()).neg_match_count + 
								GetActiveHistoryLine()->at(GetNextCrtViewActiveHistoryLine()).pos_match_count) <= best_anchor_plus_non_score)
						{							
							switch (active_history_line) {
								case 8:	crt_view_history_8++; break;
								case 0:	crt_view_history_0++; break;
								case 1:	crt_view_history_1++; break;
								case 2:	crt_view_history_2++; break;
								case 3:	crt_view_history_3++; break;
								case 4:	crt_view_history_4++; break;
								case 5:	crt_view_history_5++; break;
								case 6:	crt_view_history_6++; break;
								case 7:	crt_view_history_7++; break;

								default: crt_view_history_8++; break;
							}
						}						
					}					

					void AngleIncCrtViewActiveHistoryLine()
					{
						float crtRotation = GetActiveHistoryLine()->at(GetNextCrtViewActiveHistoryLine()).vg->rotation;

						switch (active_history_line) {
							case 8:	crt_view_history_8++; break;
							case 0:	crt_view_history_0++; break;
							case 1:	crt_view_history_1++; break;
							case 2:	crt_view_history_2++; break;
							case 3:	crt_view_history_3++; break;
							case 4:	crt_view_history_4++; break;
							case 5:	crt_view_history_5++; break;
							case 6:	crt_view_history_6++; break;
							case 7:	crt_view_history_7++; break;							

							default: crt_view_history_8++; break;
						}
						
						while ((GetNextCrtViewActiveHistoryLine() < GetActiveHistoryLine()->size()) && 
							(GetActiveHistoryLine()->at(GetNextCrtViewActiveHistoryLine()).vg->rotation - crtRotation < 0.0001f))
						{							
							switch (active_history_line) {
								case 8:	crt_view_history_8++; break;
								case 0:	crt_view_history_0++; break;
								case 1:	crt_view_history_1++; break;
								case 2:	crt_view_history_2++; break;
								case 3:	crt_view_history_3++; break;
								case 4:	crt_view_history_4++; break;
								case 5:	crt_view_history_5++; break;
								case 6:	crt_view_history_6++; break;
								case 7:	crt_view_history_7++; break;

								default: crt_view_history_8++; break;
							}
						}						
					}

					int GetNextCrtViewActiveHistoryLine()
					{
							switch (active_history_line) {
							case 8:	return crt_view_history_8;
							case 0:	return crt_view_history_0;
							case 1:	return crt_view_history_1;
							case 2:	return crt_view_history_2;
							case 3:	return crt_view_history_3;
							case 4:	return crt_view_history_4;
							case 5:	return crt_view_history_5;
							case 6:	return crt_view_history_6;
							case 7:	return crt_view_history_7;

							default: return crt_view_history_8;
						}
					}
				
					inline void UpdateIndexForBestAnchorMatches(int p_anchor_score)
					{
						if (p_anchor_score >= best_anchor_match_score)
						{
							grid_index_with_best_anchor_matches = GetNextCrtViewActiveHistoryLine();
							best_anchor_match_score = p_anchor_score;
						}
					}

					inline void UpdateIndexForBestAnchorPlusNonAnchorMatches(int p_anchor_score)
					{
						if (p_anchor_score >= best_anchor_plus_non_score)
						{
							grid_index_with_best_anchor_plus_non_anchor_matches = GetNextCrtViewActiveHistoryLine();
							best_anchor_plus_non_score = p_anchor_score;
						}
					}

					inline void UpdateIndexForBestNonAnchorMatches(int p_anchor_score)
					{
						if (p_anchor_score >= best_non_anchor_match_score)
						{
							grid_index_with_best_non_anchor_matches = GetNextCrtViewActiveHistoryLine();
							best_non_anchor_match_score = p_anchor_score;
						}
					}

					inline void UpdateIndexForBestCoverage(float p_coverage_score)
					{
						if (p_coverage_score >= best_coverage_score)
						{
							grid_index_with_best_coverage = GetNextCrtViewActiveHistoryLine();
							best_coverage_score = p_coverage_score;
						}
					}

					history_unit GetHistoryUnitWithBestCoverage()
					{
						return GetActiveHistoryLine()->at(grid_index_with_best_coverage);
					}

					void CopyOptimalGridLineToBestLine(int p_bestCoverageTrack)
					{			
						if (p_bestCoverageTrack==0) { for (auto h_unit : *m_vg_history_0) m_vg_history_8->push_back(h_unit); }
						if (p_bestCoverageTrack==1) { for (auto h_unit : *m_vg_history_1) m_vg_history_8->push_back(h_unit); }
						if (p_bestCoverageTrack==2) { for (auto h_unit : *m_vg_history_2) m_vg_history_8->push_back(h_unit); }
						if (p_bestCoverageTrack==3) { for (auto h_unit : *m_vg_history_3) m_vg_history_8->push_back(h_unit); }
						if (p_bestCoverageTrack==4) { for (auto h_unit : *m_vg_history_4) m_vg_history_8->push_back(h_unit); }
						if (p_bestCoverageTrack==5) { for (auto h_unit : *m_vg_history_5) m_vg_history_8->push_back(h_unit); }
						if (p_bestCoverageTrack==6) { for (auto h_unit : *m_vg_history_6) m_vg_history_8->push_back(h_unit); }
						if (p_bestCoverageTrack==7) { for (auto h_unit : *m_vg_history_7) m_vg_history_8->push_back(h_unit); }
						
						if (p_bestCoverageTrack!=0) for (auto h_unit : *m_vg_history_0) { Safe_Delete(h_unit.vg); }
						if (p_bestCoverageTrack!=1) for (auto h_unit : *m_vg_history_1) { Safe_Delete(h_unit.vg); }
						if (p_bestCoverageTrack!=2) for (auto h_unit : *m_vg_history_2) { Safe_Delete(h_unit.vg); }
						if (p_bestCoverageTrack!=3) for (auto h_unit : *m_vg_history_3) { Safe_Delete(h_unit.vg); }
						if (p_bestCoverageTrack!=4) for (auto h_unit : *m_vg_history_4) { Safe_Delete(h_unit.vg); }
						if (p_bestCoverageTrack!=5) for (auto h_unit : *m_vg_history_5) { Safe_Delete(h_unit.vg); }
						if (p_bestCoverageTrack!=6) for (auto h_unit : *m_vg_history_6) { Safe_Delete(h_unit.vg); }
						if (p_bestCoverageTrack!=7) for (auto h_unit : *m_vg_history_7) { Safe_Delete(h_unit.vg); }						

						m_vg_history_0->clear();
						m_vg_history_1->clear();
						m_vg_history_2->clear();
						m_vg_history_3->clear();
						m_vg_history_4->clear();
						m_vg_history_5->clear();
						m_vg_history_6->clear();
						m_vg_history_7->clear();

						//std::cout << "Adding to Best Track from " << p_bestCoverageTrack << " Best Track Size = " << m_vg_history_8->size() << std::endl;
					}

					void DeleteHistoryUnitsExceptForFdg(int fdg_id, int model_id)
					{
						std::vector<history_unit> tmp_history_line;
						for (auto h_unit : *m_vg_history_8)
						{
							if (h_unit.fdg_id == fdg_id || h_unit.model_id!=model_id)
								tmp_history_line.push_back(h_unit);
							else Safe_Delete(h_unit.vg);
						}

						m_vg_history_8->clear();
						for (auto h_unit : tmp_history_line) m_vg_history_8->push_back(h_unit);
					}

					void DeleteHistoryUnitsExceptForModelId(int model_id)
					{
						std::vector<history_unit> tmp_history_line;
						std::cout << "History Units in _8 " << m_vg_history_8->size() << std::endl;
						for (auto h_unit : *m_vg_history_8)
						{						
							if (h_unit.model_id == model_id)
								tmp_history_line.push_back(h_unit);
							else Safe_Delete(h_unit.vg);
						}
						std::cout << "Keeping " << tmp_history_line.size() << " history units " << std::endl;
						m_vg_history_8->clear();
						for (auto h_unit : tmp_history_line) m_vg_history_8->push_back(h_unit);
					}
				};

				struct vg_histories {
					std::vector<vg_history*> * histories;
					std::map<int, std::vector<SceneMapping*>*> * sceneMappings;
					int crtHistoryIndex;

					vg_histories()
					{
						histories = new std::vector<vg_history*>();
						sceneMappings = new std::map<int, std::vector<SceneMapping*>*>();
						crtHistoryIndex = 0;
					}

					bool MoreHistory() { return crtHistoryIndex < histories->size() ? true : false; }

					vg_history* GetNextHistory() { return histories->at(crtHistoryIndex); }

					void IncHistoriesIndex() { crtHistoryIndex++; }

					void ResetHistoriesIndex() { crtHistoryIndex = 0; }

					void AddHistory(vg_history * p_vg_history) { histories->push_back(p_vg_history); }

					void AddSceneMapping(int _scene, int _segment, int _model, int _fdg, int _grid)
					{
						if (sceneMappings->find(_scene) == sceneMappings->end()) (*sceneMappings)[_scene] = new std::vector<SceneMapping*>();
						(*sceneMappings)[_scene]->push_back(new SceneMapping(_scene, _segment, _model, _fdg, _grid));
					}

					std::pair<bool,SceneMapping*> GetSceneMapping(int _scene, int _segment, int _model)
					{
						if (sceneMappings->find(_scene) == sceneMappings->end()) return std::pair<bool,SceneMapping*>(false, NULL);
						bool match = false;
						SceneMapping * ret;
						std::vector<SceneMapping*> * maps = sceneMappings->at(_scene);
						for (auto m : *maps)
						{
							if (m->segment == _segment && m->model == _model)
							{
								match = true;
								ret = m;
							}
						}
						return match ? std::pair<bool,SceneMapping*>(true, ret) : std::pair<bool,SceneMapping*>(false,NULL);
					}
										
				};

				ObjectLocationConstraints limits;						//Pre-defined object constraints.
				SolutionMapping * m_preferred_solution;					//A solution consists of a map of shapes each consisiting of multiples segments with a confidence score each.
																		
				SolutionSpace * m_markovDecisionProcess;				//All solutions inferred. Non-deterministic choices are added in a tree like fashion.
				PointCloudSegmentation * m_sceneToSolve;
				ModelTraining * m_trainedModelDB;
				bool biased;

				vg_histories * histories;
				vg_history * history;
				vg_history::history_unit crt_history_element;
				std::map<int, std::vector<Vector3>*> * model_original_positions;
				int history_model_in_view;

				/****** Seeded search .. for when we want to make sure the search proceeds nicely****/
				std::map<int,std::pair<bool,std::vector<int>>> m_seeds;
				std::vector<int> m_seed_order;

				std::map<int, std::vector<int>*> * m_downsampledSegments;

				PointCloudSceneSolution(PointCloudSegmentation * p_sceneToSolve, 
										ModelTraining * p_trainedModelDB);

				int ComputeSolution();											//Enumerates the solution space and returns a number of solutions computed in an ordered list.
				SolutionMapping * GetPreferredSolution();						//Scores all solutions inferred from the root of the search space and returns the 'best' according to the reward scheme used
				void WriteIlluminPRTScript(std::string p_path);

				//**********************************************
				//**********Space Enumeration Functions*********
				int ComputeMDPSearchSpace1();																								//Compute Search Space. V1 - Pray this works - return number of states created.
				int ComputeMDPSearchSpace2();																								//Compute Search Space. V2 - Also Pray this works. - return number of states created.

				//**********************************************
				//**********Object Fitting Functions************				
				int SearchBoundaries(SolutionNode * _parent, float p_over_under_ratio, float p_min_reward, float p_reward_step);			//Fit boundaries to the parentSolution
				int SearchObjectsInScene(PointCloudSegmentation * objectPCS, SolutionNode * _parentSolution);								//Add solutionmaps for objectPCS, each assigned a probability. 
				int MatchSceneSegmentWithObjectClasses(SolutionNode * _parentSolution, 
											StructureGraph::Node * _segment, std::map<int, std::vector<SolutionMapping *>> &_mappings);		//Determine best objects which fit this (and connected/nieghbour) segments.
																																				//Determines SCORE of this segment with a specific trained model PCS instance. Returns normalised score 0->1
				std::pair<std::vector<int>, FeatureDescStructureGraph::VG_stats*> MatchSceneSegmentWithTrainedObjectVoxelGrids(SolutionNode * _parentSolution, PointCloudSegmentation * _object, StructureGraph::Node * _segment);  
				std::vector<int> GetSceneNodesConsistentWithFDGStructureString(FeatureDescStructureGraph * _fdg, StructureGraph::Node * _initialNode);      //Given a FeatureDescriptionGraph 

				//**********************************************
				//**********Sorting Functions*******************
				std::vector<int> OrderSegmentsBySize();
				std::vector<int> OrderSegmentsByConnectivity();

				//**********************************************
				//**********Logs Writing Functions**************
				void WriteSolutionToFile(std::map<int, std::vector<GraphCut*>*> * &cuts, const char * filename);
		};
	}
}